#include "LdPcl.h"

int main()
{
    //----> 变量设置 <---- //
    pcl::PassThrough<Point> pass;
    pcl::SACSegmentationFromNormals<Point, pcl::Normal> seg;
    pcl::ExtractIndices<Point> extract;
    pcl::ExtractIndices<pcl::Normal> extract_normals;
    pcl::search::KdTree<Point>::Ptr Eu_Tree(new pcl::search::KdTree<Point>());
    pcl::search::KdTree<Point>::Ptr Plane_Tree(new pcl::search::KdTree<Point>());
    pcl::NormalEstimation<Point, pcl::Normal> ne;
    // pcl::search::KdTree<Point>::Ptr tree(new pcl::search::KdTree<Point>());

    Pcloud Origin_Input(new Cloud());
    Pcloud Filter_OutPoints(new Cloud());
    Pcloud Eu_Cluster(new Cloud());
    pcl::ModelCoefficients::Ptr coefficients_plane(new pcl::ModelCoefficients()),
        coefficients_cylinder(new pcl::ModelCoefficients());
    pcl::PointIndices::Ptr inliers_Plane(new pcl::PointIndices()),
        inliers_cylinder(new pcl::PointIndices());

    //----> 读取PCD文件 / 打开雷达获取点云图<----//
    // LdsLidar &my_lidar = LdsLidar::GetInstance();
    // int ResultInitLidar = my_lidar.InitLdsLidar("auto", 3000);
    // if (!ResultInitLidar)
    // {
    //     std::cout << "Init Lidar success!" << std::endl;
    // }
    // else
    // {
    //     std::cout << "Failed" << std::endl;
    // }

    // //抓取点云图，不成功则重新抓取，直至成功
    // bool ifGet = false;
    // while (!ifGet)
    // {
    //     Origin_Input = my_lidar.GetPointCloud();
    //     sleep(2);
    //     if (Origin_Input != nullptr && Origin_Input->size() > 10000)
    //         ifGet = true;
    // }
    std::string pcd_saved = "/home/milo/Documents/visual/2Champion/PCL/data/2_3000.pcd";
    int Load_Result = pcl::io::loadPCDFile(pcd_saved, *Origin_Input);
    if (Load_Result == -1)
    {
        std::cout << "PCD load failed!" << std::endl;
        exit(-1);
    }
    // pcl::visualization::CloudViewer V_Origin("Origin_Cloud");
    // V_Origin.showCloud(Origin_Input);
    // while(!V_Origin.wasStopped()){}
    // ... //

    //----> 滤波降噪 <----//
    pcl::RadiusOutlierRemoval<Point> Point_Filter;
    Point_Filter.setInputCloud(Origin_Input);
    Point_Filter.setRadiusSearch(0.05);
    Point_Filter.setMinNeighborsInRadius(20);
    Point_Filter.filter(*Filter_OutPoints);

    pcl::visualization::CloudViewer OutPoints("OutPoints");
    OutPoints.showCloud(Filter_OutPoints);
    while (!OutPoints.wasStopped())
    {
    }
    // ... //

    //----> 直通滤波 <---- //
    //@作用: 除去地面点和远处空间点，减少后面拟合识别的计算量，同时可以提高识别准确度。
    Pcloud Pass_PCloud(new Cloud());
    Pcloud temp1(new Cloud());
    pass.setInputCloud(Filter_OutPoints);
    //离雷达的距离
    pass.setFilterFieldName("x");
    pass.setFilterLimits(2, 7);
    pass.filter(*temp1);

    //左右视野范围
    pass.setInputCloud(temp1);
    pass.setFilterFieldName("y");
    pass.setFilterLimits(-4, 4);
    pass.filter(*temp1);
    *Pass_PCloud = *temp1;
    pcl::visualization::CloudViewer PassCloud("Pass");
    PassCloud.showCloud(Pass_PCloud);
    while (!PassCloud.wasStopped())
    {
    }
    // ... //

    //----> 平面去除 <----//
    //@作用：除去滤波后的点云图中的拟合平面，为后面的欧式聚类和圆柱拟合做准备，可以减少平面的
    //      的影响，同时提高处理速度。
    std::vector<Point, Eigen::aligned_allocator<Point>>::const_iterator position;
    Cloud::iterator it;
    for (it = Pass_PCloud->begin(); it != Pass_PCloud->end(); it++)
    {
        float x = it->x;
        float y = it->y;
        float z = it->z;
        float r = sqrt(pow(x, 2) + pow(y, 2) + pow(z, 2));
        if (r >= 10)
            Pass_PCloud->erase(it);
    }
    // pcl::visualization::CloudViewer V_r("V_r");
    // V_r.showCloud(Pass_PCloud);
    // while (!V_r.wasStopped())
    // {
    // }
    //估计点的法线
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>());
    ne.setSearchMethod(Plane_Tree);
    ne.setInputCloud(Pass_PCloud);
    ne.setKSearch(50);
    ne.compute(*cloud_normals);

    //建立平面分割模型并配置参数
    seg.setOptimizeCoefficients(true); //设置为true为提取平面点，false则为非平面点
    seg.setModelType(pcl::SACMODEL_NORMAL_PLANE);
    seg.setNormalDistanceWeight(0.2);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setMaxIterations(70);
    seg.setDistanceThreshold(0.08);

    //开始一个while循环来提取多个平面
    int all_points = (int)Pass_PCloud->size();
    Pcloud Plane_Extration(new Cloud());
    while (Pass_PCloud->size() > 0.15 * all_points)
    {
        seg.setInputCloud(Pass_PCloud);
        seg.setInputNormals(cloud_normals);
        seg.segment(*inliers_Plane, *coefficients_plane);
        if (inliers_Plane->indices.size() == 0)
        {
            std::cout << "Could not estimate a planar model any more! ";
            break;
        }

        extract.setInputCloud(Pass_PCloud);
        extract.setIndices(inliers_Plane);
        extract.setNegative(false); //设置为false，可以将indices的点进行提取，就是反提取。
        extract.filter(*Plane_Extration);
        // pcl::visualization::CloudViewer Ex_Plane("Plane_Extraction");
        // Ex_Plane.showCloud(Plane_Extration);
        // while (!Ex_Plane.wasStopped())
        // {
        // }

        //将输入点云中的平面去除
        extract.setNegative(true); //设置为true，将平面外的点进行提取
        Pcloud Temp2(new Cloud());
        extract.filter(*Temp2);
        *Pass_PCloud = *Temp2;

        //获取提取剩余的法线
        pcl::PointCloud<pcl::Normal>::Ptr Temp_Normals(new pcl::PointCloud<pcl::Normal>());
        extract_normals.setNegative(true);
        extract_normals.setInputCloud(cloud_normals);
        extract_normals.setIndices(inliers_Plane);
        extract_normals.filter(*Temp_Normals);
        *cloud_normals = *Temp_Normals;
    }
    // ... //
    pcl::visualization::CloudViewer V_PlanesOut("Without All Planes");
    V_PlanesOut.showCloud(Pass_PCloud);
    while (!V_PlanesOut.wasStopped())
    {
    }

    //----> 体素滤波 <----//
    // pcl::VoxelGrid<Point> vg;
    // Pcloud Temp_Vg(new Cloud());
    // vg.setInputCloud(Pass_PCloud);
    // vg.setLeafSize(0.010f, 0.010f, 0.010f);
    // vg.filter(*Temp_Vg);

    // pcl::visualization::CloudViewer V_vg("Voxel viewer");
    // V_vg.showCloud(Temp_Vg);
    // while(!V_vg.wasStopped()){}

    // pcl::search::KdTree<Point>::Ptr VoTree(new pcl::search::KdTree<Point>());
    // ne.setSearchMethod(VoTree);
    // ne.setInputCloud(Temp_Vg);
    // ne.setKSearch(30);
    // ne.compute(*cloud_normals);
    // ... //
    int m = 0;
    //----> 进行多次多阈值的欧式聚类分割 <----//
    *Eu_Cluster = *Pass_PCloud;
    int Cylinder_Count = 0;
    for (size_t i = 0; i < 2; i++)
    {
        float threshold;
        switch (i)
        {
        case 0:
        {
            threshold = 0.012;
            break;
        }
        case 1:
        {
            threshold = 0.02;
            break;
        }
        }

        //----> 开始进行欧式聚类分割和圆柱识别 <----//
        // *Eu_Cluster = *Temp_Vg;
        Eu_Tree->setInputCloud(Eu_Cluster);
        std::vector<pcl::PointIndices> cluster_indices;
        pcl::EuclideanClusterExtraction<Point> ec;
        ec.setClusterTolerance(threshold); //点间距离，测试得到理想值
        ec.setMinClusterSize(200);
        ec.setMaxClusterSize(7000);
        ec.setSearchMethod(Eu_Tree);
        ec.setInputCloud(Eu_Cluster);
        ec.extract(cluster_indices);

        Pcloud TTimes_cloud(new Cloud());
        *TTimes_cloud = *Eu_Cluster;
        pcl::PointCloud<pcl::Normal>::Ptr TTimes_nor(new pcl::PointCloud<pcl::Normal>());
        *TTimes_nor = *cloud_normals;

        //将聚类出来的点进行剔除
        pcl::PointCloud<pcl::Normal>::Ptr Temp1_normals(new pcl::PointCloud<pcl::Normal>());
        pcl::PointIndices::Ptr temp_Indices(new pcl::PointIndices());
        // for (size_t id = 0; id < cluster_indices.size(); id++)
        // {
        //     *temp_Indices = cluster_indices.at(id);
        //     std::cout << temp_Indices->indices.size() << std::endl;

        // pcl::visualization::CloudViewer V_temp2("V_temp2");
        // V_temp2.showCloud(TTimes_cloud);
        // while(!V_temp2.wasStopped()){}
        // Pcloud stuby(new Cloud());

        extract.setInputCloud(Eu_Cluster);
        extract.setNegative(true);
        pcl::PointIndices::Ptr All_indices(new pcl::PointIndices());
        for (size_t i = 0; i < cluster_indices.size(); i++)
        {
            // *temp_Indices = cluster_indices[i];
            for (size_t j = 0; j < cluster_indices[i].indices.size(); j++)
                All_indices->indices.push_back(cluster_indices[i].indices.at(j));
        }
        extract.setIndices(All_indices);
        Pcloud Temp1_Cloud(new Cloud());
        extract.filter(*Temp1_Cloud);
        *TTimes_cloud = *Temp1_Cloud;

        // extract.filter(*stuby);
        pcl::visualization::CloudViewer extraction("Extraction");
        extraction.showCloud(TTimes_cloud);
        while (!extraction.wasStopped())
        {
        }

        // extract.setNegative(true);

        std::cout << "the num of cloud points is: " << TTimes_cloud->size() << std::endl;
        // pcl::visualization::CloudViewer V_Temp1("V_Temp1");
        // V_Temp1.showCloud(TTimes_cloud);
        // while (!V_Temp1.wasStopped())
        // {
        // }

        extract_normals.setNegative(true);
        extract_normals.setInputCloud(TTimes_nor);
        extract_normals.setIndices(temp_Indices);
        extract_normals.filter(*Temp1_normals);
        *TTimes_nor = *Temp1_normals;
        // }

        std::cout << "Cluster things amount: " << cluster_indices.size() << std::endl;

        //在for循环中，将每一个聚类出来的物体进行分割提取
        for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin();
             it != cluster_indices.end();
             it++)
        {
            //用indices获取每一个聚类物体的点，存到一个新的点云中，并进行圆柱拟合。
            Pcloud Temp_cluster(new Cloud());

            for (const auto &idx : it->indices)
                Temp_cluster->push_back((*Eu_Cluster)[idx]);

            Temp_cluster->width = Temp_cluster->size();
            Temp_cluster->height = 1;
            Temp_cluster->is_dense = true;

            std::cout << "PointCloud representing the Cluster: " << Temp_cluster->size() << "data points" << std::endl;

            pcl::visualization::CloudViewer V_Cluster("Cluster view");
            V_Cluster.showCloud(Temp_cluster);
            while (!V_Cluster.wasStopped())
            {
            }

            //开始进行圆柱拟合。
            //通过indices获取聚类物体的法线
            pcl::PointIndices::Ptr Temp_Indices(new pcl::PointIndices());
            *Temp_Indices = *it;
            pcl::PointCloud<pcl::Normal>::Ptr Cluster_normals(new pcl::PointCloud<pcl::Normal>());
            extract_normals.setNegative(false);
            extract_normals.setInputCloud(cloud_normals);
            extract_normals.setIndices(Temp_Indices);
            extract_normals.filter(*Cluster_normals);

            seg.setOptimizeCoefficients(true);
            seg.setModelType(pcl::SACMODEL_CYLINDER);
            seg.setMethodType(pcl::SAC_RANSAC);
            seg.setNormalDistanceWeight(0.5);
            seg.setMaxIterations(10000);
            seg.setDistanceThreshold(0.05);
            seg.setRadiusLimits(0.085, 0.5);
            seg.setInputCloud(Temp_cluster);
            seg.setInputNormals(Cluster_normals);

            //获取圆柱内点和参数
            seg.segment(*inliers_cylinder, *coefficients_cylinder);

            //把圆柱分割出来
            extract.setNegative(false);
            extract.setInputCloud(Temp_cluster);
            extract.setIndices(inliers_cylinder);
            Pcloud Cylinder_cloud(new Cloud());
            extract.filter(*Cylinder_cloud);
            if (Cylinder_cloud->points.empty())
            {
                std::cerr << "Could not find a Cylinder in the Eu cluser! " << std::endl;
                continue;
            }
            std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;
            float x = 0, y = 0, z = 0;
            for (size_t j = 0; j < Cylinder_cloud->size(); j++)
            {
                x += Cylinder_cloud->at(j).x;
                y += Cylinder_cloud->at(j).y;
                z += Cylinder_cloud->at(j).y;
            }
            x /= Cylinder_cloud->size();
            y /= Cylinder_cloud->size();
            z /= Cylinder_cloud->size();

            // pcl::visualization::CloudViewer V_Cylinder("Cylidner view");
            // V_Cylinder.showCloud(Cylinder_cloud);
            // while (!V_Cylinder.wasStopped())
            // {
            // }
            //将识别到的轴线画在原始点云图中
            Point p1, p2, p3;
            Point v;
            v.x = (*coefficients_cylinder).values[3];
            v.y = (*coefficients_cylinder).values[4];
            v.z = (*coefficients_cylinder).values[5];

            p1.x = (*coefficients_cylinder).values[0];
            p1.y = (*coefficients_cylinder).values[1];
            p1.z = (*coefficients_cylinder).values[2];

            float dx = p1.x - v.x;
            float dy = p1.y - v.y;
            float dz = p1.z - v.z;
            float result = dx * v.x +
                           dy * v.y +
                           dz * v.z;

            if (result < 0)
            {
                v.x = -v.x;
                v.y = -v.y;
                v.z = -v.z;
            }
            //将起始点转换到地面上的点
            // float dz = 0 - p1.z;
            // float k = dz / (*coefficients_cylinder).values[5];
            // p1.x += k * (*coefficients_cylinder).values[3];
            // p1.y += k * (*coefficients_cylinder).values[4];
            // p1.z = 0;
            for (size_t i = 0; i < 1000; i++)
            {
                p2.x = p1.x + i * 0.01 * v.x;
                p2.y = p1.y + i * 0.01 * v.y;
                p2.z = p1.z + i * 0.01 * v.z;

                p3.x = p1.x - i * 0.01 * v.x;
                p3.y = p1.y - i * 0.01 * v.y;
                p3.z = p1.z - i * 0.01 * v.z;
                Origin_Input->points.push_back(p2);
                // Origin_Input->points.push_back(p3);
            }
            Cylinder_Count++;
        }

        *Eu_Cluster = *TTimes_cloud;
        *cloud_normals = *TTimes_nor;
        pcl::visualization::CloudViewer V_Eu_times("Every time Eu");
        V_Eu_times.showCloud(Eu_Cluster);
        while (!V_Eu_times.wasStopped())
        {
        }
    }
    pcl::visualization::CloudViewer V_All_Cylidner("After all segmentaion");
    V_All_Cylidner.showCloud(Origin_Input);
    while (!V_All_Cylidner.wasStopped())
    {
    }
    std::cout << "Found Cylinders amount: " << Cylinder_Count << std::endl;
    return 0;
}

